import sys
import threading

import pygame

output_mod = False

# 最大速度设置
speed = 60


class Mission:
    control = None
    tool = None

    mission_numb = 0

    # 自动任务开关
    control_switch = True
    control_thread = None

    send_switch = False
    send_thread = None

    on_fly = False

    roll = 0
    pitch = 0
    throttle = 0
    yaw = 0

    screen = None

    def __init__(self, control=None, tool=None):
        self.control = control
        self.tool = tool

    def control_t(self):
        """
        自动任务线程
        :return:
        """
        while True:
            if self.control_switch:
                if self.mission_numb == 0:
                    self.control.takeoff()
                    self.control.wait()
                    self.on_fly = True
                    self.mission_numb += 1
                elif self.mission_numb == 1:
                    dy = self.tool.get_Y_move()
                    if dy > 0:
                        self.control.up(dy)
                    else:
                        self.control.down(-dy)
                    self.control.wait()
                    dx = self.tool.get_X_move()
                    if dx > 0:
                        self.control.up(dy)
                    else:
                        self.control.up(-dy)
                    self.control.wait()
                    self.control.forward(self.tool.get_D_move() + 30)
                    self.control.wait()
                    self.mission_numb += 1
                elif self.mission_numb == 2:
                    self.control.land()
                    self.control.wait()
                    self.mission_numb += 1

    def start(self):

        if self.send_thread is None:
            self.send_thread = threading.Thread(target=self.send)
        self.send_thread.start()

        if self.control_thread is None:
            self.control_thread = threading.Thread(target=self.control_t)
        self.control_thread.start()

        # 初始化 Pygame
        pygame.init()

        # 设置窗口大小
        self.screen = pygame.display.set_mode((400, 300))
        while True:
            for event in pygame.event.get():
                if event.type == pygame.QUIT:
                    pygame.quit()
                    sys.exit()
                elif event.type == pygame.KEYDOWN:
                    if event.key == pygame.K_w:
                        self.pitch = speed
                        if output_mod:
                            print("按下了 w 键！")
                    elif event.key == pygame.K_a:
                        self.roll = -speed
                        if output_mod:
                            print("按下了 a 键！")
                    elif event.key == pygame.K_s:
                        self.pitch = -speed
                        if output_mod:
                            print("按下了 s 键！")
                    elif event.key == pygame.K_d:
                        self.roll = speed
                        if output_mod:
                            print("按下了 d 键！")
                    elif event.key == pygame.K_SPACE:
                        self.throttle = speed
                        if output_mod:
                            print("按下了空格键！")
                    elif event.key == pygame.K_LSHIFT or event.key == pygame.K_RSHIFT:
                        self.throttle = -speed
                        if output_mod:
                            print("按下了 Shift 键！")
                    elif event.key == pygame.K_LEFT:
                        self.yaw = -100
                        if output_mod:
                            print("按下了左箭头键！")
                    elif event.key == pygame.K_RIGHT:
                        self.yaw = 100
                        if output_mod:
                            print("按下了右箭头键！")
                elif event.type == pygame.KEYUP:
                    if event.key == pygame.K_w:
                        self.pitch = 0
                        if output_mod:
                            print("松开了 w 键！")
                    elif event.key == pygame.K_a:
                        self.roll = 0
                        if output_mod:
                            print("松开了 a 键！")
                    elif event.key == pygame.K_s:
                        self.pitch = 0
                        if output_mod:
                            print("松开了 s 键！")
                    elif event.key == pygame.K_d:
                        self.roll = 0
                        if output_mod:
                            print("松开了 d 键！")
                    elif event.key == pygame.K_SPACE:
                        self.throttle = 0
                        if output_mod:
                            print("松开了空格键！")
                    elif event.key == pygame.K_LSHIFT or event.key == pygame.K_RSHIFT:
                        self.throttle = 0
                        if output_mod:
                            print("松开了 Shift 键！")
                    elif event.key == pygame.K_LEFT:
                        self.yaw = 0
                        if output_mod:
                            print("松开了左箭头键！")
                    elif event.key == pygame.K_RIGHT:
                        self.yaw = 0
                        if output_mod:
                            print("松开了右箭头键！")
                    elif event.key == pygame.K_t:
                        if output_mod:
                            print("松开了t键！")
                        self.control.takeoff()
                        self.control.wait()
                        self.on_fly = True
                    elif event.key == pygame.K_y:
                        if output_mod:
                            print("松开了y键！")
                        self.control.land()
                        self.control.wait()
                        self.on_fly = False
                    elif event.key == pygame.K_o:
                        self.send_switch = True
                        self.control_switch = False
                        if output_mod:
                            print("松开了o键！")
                    elif event.key == pygame.K_p:
                        self.send_switch = False
                        self.control_switch = True
                        if output_mod:
                            print("松开了p键！")

            # if self.send_switch and self.on_fly:
            #     self.control.set_rc(self.roll, self.pitch, self.throttle, self.yaw)

    def send(self):
        while True:
            if self.send_switch and self.on_fly:
                self.control.set_rc(self.roll, self.pitch, self.throttle, self.yaw)
